#include <Mint/Keymap.h>
#include "Kernel.h"
#include "Drivers/Keyboard.h"
#include "Utils/String.h"
#include "Hal/IHardware.h"
#include "Hal/x86/Hardware.h"
#include "Hal/x86/CPU.h"
#include "Scheduler/Scheduler.h"

using namespace x86;

enum
{
	KEYBOARD_INTERRUPT = 0x21,
	KEYBOARD_STATUS_PORT = 0x64,
	KEYBOARD_DATA_PORT = 0x60,

	KEYBOARD_SCANCODE_RELEASED = 0x80,
	KEYBOARD_SCANCODE_ESCAPE_0 = 0xE0,
	KEYBOARD_SCANCODE_ESCAPE_1 = 0xE1,
	KEYBOARD_SCANCODE_DOUBLE_BYTE = 0xF0,

	KEYBOARD_COMMAND_SET_LEDS = 0xED,
	KEYBOARD_COMMAND_ECHO = 0xEE,
	KEYBOARD_COMMAND_SCANCODE_SET = 0xF0,
	KEYBOARD_COMMAND_IDENTIFY = 0xF2,
	KEYBOARD_COMMAND_TYPEMATIC_RATE = 0xF3,
	KEYBOARD_COMMAND_ENABLE = 0xF4,
	KEYBOARD_COMMAND_DISABLE = 0xF5,
	KEYBOARD_COMMAND_RESEND = 0xFE,
	KEYBOARD_COMMAND_RESET = 0xFF,

	KEYBOARD_MESSAGE_ACKNOWLEDGE = 0xFA,
	KEYBOARD_MESSAGE_SELF_TEST_PASSED = 0xAA,
	KEYBOARD_MESSAGE_ECHO_COMMAND = 0xEE,
	KEYBOARD_MESSAGE_RESEND = 0xFE,
	KEYBOARD_MESSAGE_ERROR = 0xFF,

	KEYBOARD_HELD_LEFT_SHIFT = 0x0001,
	KEYBOARD_HELD_RIGHT_SHIFT = 0x0002,
	KEYBOARD_HELD_LEFT_CONTROL = 0x0004,
	KEYBOARD_HELD_RIGHT_CONTROL = 0x0008,
	KEYBOARD_HELD_LEFT_ALT = 0x0010,
	KEYBOARD_HELD_RIGHT_ALT = 0x0020,
	KEYBOARD_HELD_LEFT_SUPER = 0x0040,
	KEYBOARD_HELD_RIGHT_SUPER = 0x0080,

	KEYBOARD_HELD_SHIFT = 0x0001 | 0x0002,
	KEYBOARD_HELD_CONTROL = 0x0004 | 0x0008,
	KEYBOARD_HELD_ALT = 0x0010 | 0x0020,
	KEYBOARD_HELD_SUPER = 0x0040 | 0x0080,

	KEYBOARD_MODIFIER_ESCAPE_0 = 0x1000,
	KEYBOARD_MODIFIER_ESCAPE_1 = 0x2000,
	KEYBOARD_MODIFIER_DOUBLE_BYTE = 0x4000,

	KEYBOARD_LED_SCROLL_LOCK = 0x01,
	KEYBOARD_LED_NUM_LOCK = 0x02,
	KEYBOARD_LED_CAPS_LOCK = 0x04,
};

CKeyboard sKeyboard;
CKeyboard *

Result CKeyboard::Initialize( uint16 * keymap )
{
	KEYBOARD_TRACE( "Keyboard initializing...\n" );

	fReadPos = fWritePos = 0;
	fHeld = fModifiers = 0;
	fLEDs = 0;
	fKeymap = keymap;

#if 1
	Memory_Set( &fEvents, 0, sizeof(fEvents) );
#else
	for( int i = 0; i < KEYBOARD_MAX_EVENTS; i++ )
	{
		fEvents[i].fKeyCode = 0;
		fEvents[i].fModifiers = 0;
	}
#endif

	// register IRQ handler
	sHardware->AddInterruptHandler( KEYBOARD_INTERRUPT, &__HandleInterrupt );

	// reset the keyboard
	_SendCommand( KEYBOARD_COMMAND_RESET );
	_SendCommand( KEYBOARD_COMMAND_DISABLE );
	_SendCommand( KEYBOARD_COMMAND_ENABLE );

	return RESULT_OK;
}

char CKeyboard::Read( void )
{
    while( HasEvent() != true )
        Scheduler::Yield();

    KeyboardEvent_t event;
    ReadEvent( &event );

    return event.fKeyCode;
}


void CKeyboard::ReadEvent( KeyboardEvent_t * event )
{
	if( event == NULL )
		return;

	event->fKeyCode = fEvents[fReadPos].fKeyCode;
	event->fModifiers = fEvents[fReadPos].fModifiers;

	if( fReadPos != fWritePos )
		fReadPos = ++fReadPos % KEYBOARD_MAX_EVENTS;
}


void CKeyboard::PeekEvent( KeyboardEvent_t * event )
{
	if( event == NULL )
		return;

	event->fKeyCode = fEvents[fReadPos].fKeyCode;
	event->fModifiers = fEvents[fReadPos].fModifiers;
}


void CKeyboard::WriteEvent( KeyboardEvent_t * event )
{
	if( event == NULL )
		return;

	fEvents[fWritePos].fKeyCode = event->fKeyCode;
	fEvents[fWritePos].fModifiers = event->fModifiers;

	fWritePos = ++fWritePos % KEYBOARD_MAX_EVENTS;

	if( fWritePos == fReadPos )
		fReadPos = ++fReadPos % KEYBOARD_MAX_EVENTS;
}


void CKeyboard::_WaitUntilReady( void )
{
	while( FLAG_PRESENT( IO_In8( KEYBOARD_STATUS_PORT ), 0x02 ) )
		IO_Delay();
}


void CKeyboard::_SendCommand( uint8 command )
{
	KEYBOARD_TRACE( "Sending keyboard the command %2x...\n", command );

	while( true )
	{
		KEYBOARD_TRACE( "Waiting for keyboard to be ready...\n" );
		_WaitUntilReady();

		IO_Out8( KEYBOARD_DATA_PORT, command );
		KEYBOARD_TRACE( "Command sent...\n" );

		KEYBOARD_TRACE( "Waiting for keyboard to be ready...\n" );
		_WaitUntilReady();

		uint8 reply = IO_In8( KEYBOARD_DATA_PORT );
		KEYBOARD_TRACE( "Command reply %2x...\n", reply );

		if( reply == KEYBOARD_MESSAGE_ACKNOWLEDGE )
		{
			KEYBOARD_TRACE( "Command acknowledged...\n" );
			break;
		}
		else
			KEYBOARD_TRACE( "Command NOT acknowledged, retrying...\n" );
	}
}


void CKeyboard::_SendCommand( uint8 command, uint8 value )
{
	_SendCommand( command );
	IO_Out8( KEYBOARD_DATA_PORT, value );
}


void CKeyboard::__HandleInterrupt( ITaskContext * context )
{
	sKeyboard._HandleInterrupt( context );
}

void CKeyboard::_HandleInterrupt( ITaskContext * context )
{
	if( fKeymap == NULL )
	{
		KEYBOARD_TRACE( "Interrupt received, but we don't have a keymap... ignored.\n" );
		return;
	}

	uint16 scanCode = IO_In8( KEYBOARD_DATA_PORT );

	if( FLAG_PRESENT( fModifiers, KEYBOARD_MODIFIER_DOUBLE_BYTE ) )
	{
		scanCode |= IO_In8( KEYBOARD_DATA_PORT ) << 8;
		FLAG_CLEAR( fModifiers, KEYBOARD_MODIFIER_DOUBLE_BYTE );
	}

	KEYBOARD_TRACE( "Interrupt received. Scancode=%04x\n", scanCode );

	switch( scanCode )
	{
	case KEYBOARD_SCANCODE_ESCAPE_0:
		FLAG_SET( fModifiers, KEYBOARD_MODIFIER_ESCAPE_0 );
		return;

	case KEYBOARD_SCANCODE_ESCAPE_1:
		FLAG_SET( fModifiers, KEYBOARD_MODIFIER_ESCAPE_1 );
		return;

	case KEYBOARD_SCANCODE_DOUBLE_BYTE:
		FLAG_SET( fModifiers, KEYBOARD_MODIFIER_DOUBLE_BYTE );
		return;
	}

	bool pressed = !FLAG_PRESENT( scanCode, KEYBOARD_SCANCODE_RELEASED );
	FLAG_CLEAR( scanCode, KEYBOARD_SCANCODE_RELEASED );

	uint16 * keymap = NULL;

	if( FLAG_ANY_PRESENT( fModifiers, KEYBOARD_MODIFIER_ESCAPE_0 | KEYBOARD_MODIFIER_ESCAPE_1 ) )
		keymap = (uint16 *)( (Address)fKeymap + KEYMAP_EXTENDED * KEYMAP_SECTION_SIZE * sizeof(uint16) );

	else if( FLAG_PRESENT( fModifiers, KEYBOARD_MODIFIER_SHIFT ) )
		keymap = (uint16 *)( (Address)fKeymap + KEYMAP_SHIFT * KEYMAP_SECTION_SIZE * sizeof(uint16) );

	else
		keymap = (uint16 *)( (Address)fKeymap + KEYMAP_NORMAL * KEYMAP_SECTION_SIZE * sizeof(uint16) );

	uint16 keyCode = keymap[(uint8)scanCode];

	if( IsAlpha( keyCode ) && FLAG_PRESENT( fModifiers, KEYBOARD_MODIFIER_CAPS_LOCK ) )
		keyCode = SwitchCase( keyCode );

	switch( keyCode )
	{
	case KEY_LEFT_SHIFT:
		if( pressed )
		{
			FLAG_SET( fHeld, KEYBOARD_HELD_LEFT_SHIFT );
			FLAG_SET( fModifiers, KEYBOARD_MODIFIER_SHIFT );
		}
		else
		{
			FLAG_CLEAR( fHeld, KEYBOARD_HELD_LEFT_SHIFT );
			if( !FLAG_ANY_PRESENT( fHeld, KEYBOARD_HELD_SHIFT ) )
				FLAG_CLEAR( fModifiers, KEYBOARD_MODIFIER_SHIFT );
		}
		break;

	case KEY_RIGHT_SHIFT:
		if( pressed )
		{
			FLAG_SET( fHeld, KEYBOARD_HELD_RIGHT_SHIFT );
			FLAG_SET( fModifiers, KEYBOARD_MODIFIER_SHIFT );
		}
		else
		{
			FLAG_CLEAR( fHeld, KEYBOARD_HELD_RIGHT_SHIFT );
			if( !FLAG_ANY_PRESENT( fHeld, KEYBOARD_HELD_SHIFT ) )
				FLAG_CLEAR( fModifiers, KEYBOARD_MODIFIER_SHIFT );
		}
		break;

	case KEY_LEFT_CONTROL:
		if( pressed )
		{
			FLAG_SET( fHeld, KEYBOARD_HELD_LEFT_CONTROL );
			FLAG_SET( fModifiers, KEYBOARD_MODIFIER_CONTROL );
		}
		else
		{
			FLAG_CLEAR( fHeld, KEYBOARD_HELD_LEFT_CONTROL );
			if( !FLAG_ANY_PRESENT( fHeld, KEYBOARD_HELD_CONTROL ) )
				FLAG_CLEAR( fModifiers, KEYBOARD_MODIFIER_CONTROL );
		}
		break;

	case KEY_RIGHT_CONTROL:
		if( pressed )
		{
			FLAG_SET( fHeld, KEYBOARD_HELD_RIGHT_CONTROL );
			FLAG_SET( fModifiers, KEYBOARD_MODIFIER_CONTROL );
		}
		else
		{
			FLAG_CLEAR( fHeld, KEYBOARD_HELD_RIGHT_CONTROL );
			if( !FLAG_ANY_PRESENT( fHeld, KEYBOARD_HELD_CONTROL ) )
				FLAG_CLEAR( fModifiers, KEYBOARD_MODIFIER_CONTROL );
		}
		break;

	case KEY_LEFT_ALT:
		if( pressed )
		{
			FLAG_SET( fHeld, KEYBOARD_HELD_LEFT_ALT );
			FLAG_SET( fModifiers, KEYBOARD_MODIFIER_ALT );
		}
		else
		{
			FLAG_CLEAR( fHeld, KEYBOARD_HELD_LEFT_ALT );
			if( !FLAG_ANY_PRESENT( fHeld, KEYBOARD_HELD_ALT ) )
				FLAG_CLEAR( fModifiers, KEYBOARD_MODIFIER_ALT );
		}
		break;

	case KEY_RIGHT_ALT:
		if( pressed )
		{
			FLAG_SET( fHeld, KEYBOARD_HELD_RIGHT_ALT );
			FLAG_SET( fModifiers, KEYBOARD_MODIFIER_ALT );
		}
		else
		{
			FLAG_CLEAR( fHeld, KEYBOARD_HELD_RIGHT_ALT );
			if( !FLAG_ANY_PRESENT( fHeld, KEYBOARD_HELD_ALT ) )
				FLAG_CLEAR( fModifiers, KEYBOARD_MODIFIER_ALT );
		}
		break;

	case KEY_LEFT_SUPER:
		if( pressed )
		{
			FLAG_SET( fHeld, KEYBOARD_HELD_LEFT_SUPER );
			FLAG_SET( fModifiers, KEYBOARD_MODIFIER_SUPER );
		}
		else
		{
			FLAG_CLEAR( fHeld, KEYBOARD_HELD_LEFT_SUPER );
			if( !FLAG_ANY_PRESENT( fHeld, KEYBOARD_HELD_SUPER ) )
				FLAG_CLEAR( fModifiers, KEYBOARD_MODIFIER_SUPER );
		}
		break;

	case KEY_RIGHT_SUPER:
		if( pressed )
		{
			FLAG_SET( fHeld, KEYBOARD_HELD_RIGHT_SUPER );
			FLAG_SET( fModifiers, KEYBOARD_MODIFIER_SUPER );
		}
		else
		{
			FLAG_CLEAR( fHeld, KEYBOARD_HELD_RIGHT_SUPER );
			if( !FLAG_ANY_PRESENT( fHeld, KEYBOARD_HELD_SUPER ) )
				FLAG_CLEAR( fModifiers, KEYBOARD_MODIFIER_SUPER );
		}
		break;

	case KEY_MENU:
		if( pressed )
			FLAG_SET( fModifiers, KEYBOARD_MODIFIER_MENU );
		else
			FLAG_CLEAR( fModifiers, KEYBOARD_MODIFIER_MENU );
		break;

	case KEY_CAPS_LOCK:
		if( pressed )
		{
			if( FLAG_PRESENT( fModifiers, KEYBOARD_MODIFIER_CAPS_LOCK ) )
				FLAG_CLEAR( fModifiers, KEYBOARD_MODIFIER_CAPS_LOCK );
			else
				FLAG_SET( fModifiers, KEYBOARD_MODIFIER_CAPS_LOCK );
		}
		break;

	case KEY_SCROLL_LOCK:
		if( pressed )
		{
			if( FLAG_PRESENT( fModifiers, KEYBOARD_MODIFIER_SCROLL_LOCK ) )
				FLAG_CLEAR( fModifiers, KEYBOARD_MODIFIER_SCROLL_LOCK );
			else
				FLAG_SET( fModifiers, KEYBOARD_MODIFIER_SCROLL_LOCK );
		}
		break;

	case KEY_PAD_NUM_LOCK:
		if( pressed )
		{
			if( FLAG_PRESENT( fModifiers, KEYBOARD_MODIFIER_NUM_LOCK ) )
				FLAG_CLEAR( fModifiers, KEYBOARD_MODIFIER_NUM_LOCK );
			else
				FLAG_SET( fModifiers, KEYBOARD_MODIFIER_NUM_LOCK );
		}
		break;
	}

	KeyboardEvent_t event;
	event.fKeyCode = keyCode;
	event.fModifiers = fModifiers;

	if( pressed )
		FLAG_SET( event.fModifiers, KEYBOARD_MODIFIER_PRESSED );

	Keyboard::WriteEvent( &event );

	int8 leds = 0;

	if( FLAG_PRESENT( fModifiers, KEYBOARD_MODIFIER_SCROLL_LOCK ) )
		FLAG_SET( leds, KEYBOARD_LED_SCROLL_LOCK );

	if( FLAG_PRESENT( fModifiers, KEYBOARD_MODIFIER_NUM_LOCK ) )
		FLAG_SET( leds, KEYBOARD_LED_NUM_LOCK );

	if( FLAG_PRESENT( fModifiers, KEYBOARD_MODIFIER_CAPS_LOCK ) )
		FLAG_SET( leds, KEYBOARD_LED_CAPS_LOCK );

	if( leds != fLEDs )
	{
		_SendCommand( KEYBOARD_COMMAND_SET_LEDS, leds );
		fLEDs = leds;
	}
}

